/*
 * Altitude detection by barometric pressure sensing.
 *
 * In Flying Fox this is based on the Hope RF HP03 pressure sensor
 *
 * by Zik Saleeba 2008-02-12
 */

#include <string.h>
#include <math.h>

#include "ffox.h"
#include "twi.h"
#include "i2c.h"
#include "AT91SAM7X256.h"
#include "lib_AT91SAM7X256.h"
#include "hp03.h"
#include "dbgprintf.h"

/* local defines */
#define HP03_MCLK_HZ 32768
#define HP03_I2C_SENSOR_ADDRESS 0x77
#define HP03_I2C_CALIBRATION_ADDRESS 0x50
#define HP03_MEASURE_COMMAND 0xff
#define HP03_RESULT_COMMAND 0xfd
#define HP03_PRESSURE_SUBTYPE 0xf0
#define HP03_TEMPERATURE_SUBTYPE 0xe8
#define HP03_RESULT_SUBTYPE 0xef
#define HP03_NUM_COEFF 7
#define HP03_MAX_PRESSURE 1100      // in Hpa
#define HP03_PRESSURE_ARRAY_MAX 80  // number of entries in pressure array
#define HP03_PRESSURE_INC 10.0      // 10Hpa between array values

#define GET_BIGENDIAN_WORD(x, y) (((unsigned short)x[y] << 8) | x[y+1])


/* module variables */
static int HP03_GotCalibrationData = false;

static double HP03_Calibration_C1;
static double HP03_Calibration_C2_Mod;
static double HP03_Calibration_C3_Mod;
static double HP03_Calibration_C4_Mod;
static double HP03_Calibration_C5;
static double HP03_Calibration_C6_Mod;
static double HP03_Calibration_C7_Mod;
static double  HP03_Calibration_AC;
static double  HP03_Calibration_BC;
static double  HP03_Calibration_D_Mod;


/* prototypes */
double HP03_GetTemperatureFactor(void);
double HP03_CalcTemperature(double dUT);
double HP03_CalcAltitude(double Pressure);


/*
 * NAME:        HP03_Init
 * ACTION:      Initialise the HP03 sensor. This sets up the 32768Hz output as well.
 */
 
void HP03_Init( void )
{
    /* set up TIMER1 to 32768Hz with output on TIOA1 */
    AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | 
                             AT91C_TC_BURST_NONE | 
                             AT91C_TC_WAVESEL_UP_AUTO |
                             AT91C_TC_CPCTRG |
                             AT91C_TC_WAVE |
                             AT91C_TC_ACPA_SET |
                             AT91C_TC_ACPC_CLEAR |
                             AT91C_TC_ASWTRG_CLEAR;
    
    AT91C_BASE_TC1->TC_CV = 0;
    AT91C_BASE_TC1->TC_RC = MCK/2 / HP03_MCLK_HZ;           /* the period */
    AT91C_BASE_TC1->TC_RA = MCK/2 / HP03_MCLK_HZ / 2;       /* TIOA1 goes high at half the period */
    AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
    
    /* disable all timer interrupts */
    AT91C_BASE_TC1->TC_IDR = AT91C_TC_COVFS | AT91C_TC_LOVRS | 
            AT91C_TC_CPAS | AT91C_TC_CPBS | AT91C_TC_CPCS |
            AT91C_TC_LDRAS | AT91C_TC_LDRBS | AT91C_TC_ETRGS;
	
    /* set XCLR to its default state of 0 on PA2 */
	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, HP03D_XCLR);
}



/*
 * NAME:        HP03_GetCalibrationData
 * ACTION:      Get the calibration data from the HP03 if we
 *              don't already have it. This can't be done in
 *              initialisation since we need to have the full
 *              multi-tasking system up and running first.
 */
 
void HP03_GetCalibrationData()
{
    unsigned char CalData[18];      /* calibration raw data */
    int I2CResult;

    /* read the calibration values */
	//dbgprintf("getting calibration values\r\n");
	memset(CalData, '\0', sizeof(CalData));
    I2CResult = I2C_Send_Command(HP03_I2C_CALIBRATION_ADDRESS, 16);        /* calibration values start at address 16 */
	if (I2CResult == I2C_ERROR_OK)
	{
	    I2CResult = I2C_Transfer(HP03_I2C_CALIBRATION_ADDRESS, NULL, 0, CalData, sizeof(CalData));
	}
	
    if (I2CResult != I2C_ERROR_OK)
    {
	    dbgprintf("bad calibration command response: %s\r\n", (I2CResult == I2C_ERROR_BUSY) ? "I2C_ERROR_BUSY" : "I2C_ERROR_TIMEOUT");
	}

#if 0
    {
        int Count;
        dbgprintf("got calibration values:");
    
    	/* store the calibration factors */
    	for (Count = 0; Count < sizeof(CalData); Count++)
    	    dbgprintf(" %02x", CalData[Count]);
    	dbgprintf("\r\n");
    	    
    	for (Count = 0; Count < HP03_NUM_COEFF; Count++)
    	{
    	    dbgprintf(" %04x (%d)", GET_BIGENDIAN_WORD(CalData, Count*2), GET_BIGENDIAN_WORD(CalData, Count*2));
    	}
    	dbgprintf("\r\n");
    }
#endif
	
	HP03_Calibration_C1 = GET_BIGENDIAN_WORD(CalData, 0);
	HP03_Calibration_C2_Mod = GET_BIGENDIAN_WORD(CalData, 2) * 4.0;
	HP03_Calibration_C3_Mod = GET_BIGENDIAN_WORD(CalData, 4) / 1024.0;
	HP03_Calibration_C4_Mod = (GET_BIGENDIAN_WORD(CalData, 6) - 1024.0) / 4096.0;
	HP03_Calibration_C5 = GET_BIGENDIAN_WORD(CalData, 8);
	HP03_Calibration_C6_Mod = GET_BIGENDIAN_WORD(CalData, 10) / 65536.0;
	HP03_Calibration_C7_Mod = GET_BIGENDIAN_WORD(CalData, 12) / 10.0;

	HP03_Calibration_AC = (double)CalData[14] / (1 << CalData[16]);
	HP03_Calibration_BC = (double)CalData[15] / (1 << CalData[16]);
	HP03_Calibration_D_Mod = (double)(1 << CalData[17]);
	
	HP03_GotCalibrationData = true;
}


/*
 * NAME:        HP03_I2C_Measure
 * ACTION:      Start a measurement on the HP03
 * RETURNS:     I2C_ERROR_OK on success, I2C_ERROR_* on error
 */
 
int HP03_I2C_Measure(unsigned char SubType)
{
    unsigned char TxBuf[2];
    
    TxBuf[0] = HP03_MEASURE_COMMAND;
    TxBuf[1] = SubType;
    
    return I2C_Transfer(HP03_I2C_SENSOR_ADDRESS, TxBuf, sizeof(TxBuf), NULL, 0);
} 


/*
 * NAME:        HP03_I2C_Result
 * ACTION:      Get a measurement result from the HP03
 * RETURNS:     I2C_ERROR_OK on success, I2C_ERROR_* on error
 */
 
int HP03_I2C_Result(unsigned short *Result)
{
    unsigned char TxBuf[2];
    unsigned char RxBuf[2];
    int Status;
    
    TxBuf[0] = HP03_RESULT_COMMAND;
    TxBuf[1] = HP03_RESULT_SUBTYPE;
    
    Status = I2C_Transfer(HP03_I2C_SENSOR_ADDRESS, TxBuf, sizeof(TxBuf), RxBuf, sizeof(RxBuf));
    
    *Result = (RxBuf[0] << 8) | RxBuf[1];
    
    return Status;
} 


/*
 * NAME:        HP03_GetAltitude
 * ACTION:      Get the current altitude by reading the barometric pressure
 * PARAMETERS:  double *Temperature - optionally the temperature can be placed here.
 *                  If non-NULL, set to the temperature in tenths of degrees celcius
 * RETURNS:     double - the altitude in metres of elevation above sea level
 *                  or ALT_ERROR on error.
 */
 
double HP03_GetAltitude(double *Temperature)
{
    unsigned short AltitudeReading;
    double Off;
    double Sens;
    double X;
    double Pressure;
    double dUT;
    int Result;
    
    /* make sure we've got the calibration data */
    if (!HP03_GotCalibrationData)
        HP03_GetCalibrationData();

    /* get the temperature first */
    dUT = HP03_GetTemperatureFactor();
	if (dUT == ALT_ERROR)
	    return ALT_ERROR;
    
    if (Temperature != NULL)
    {
        /* return the actual temperature */
        *Temperature = HP03_CalcTemperature(dUT);
    }
    
    /* raise XCLR to enable measurement */
	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, HP03D_XCLR);
    
    /* send pressure measurement command */
    if (HP03_I2C_Measure(HP03_PRESSURE_SUBTYPE) != I2C_ERROR_OK)
    {
        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, HP03D_XCLR);
        return 0;
    }
        
    /* wait for 40ms */
    vTaskDelay(40/portTICK_RATE_MS);
    
    /* request result */
    Result = HP03_I2C_Result(&AltitudeReading);
    
    /* drop XCLR */
	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, HP03D_XCLR);
	
	/* fail if we need to */
    if (Result != I2C_ERROR_OK)
        return 0;
	
	/* calculate the result */
	/* Off = (C2 + (C4 - 1024) * dUT / 2^14) * 4 */
	Off = HP03_Calibration_C2_Mod + HP03_Calibration_C4_Mod * dUT;
	
	/* Sens = C1 + C3 * dUT/2^10 */
	Sens = HP03_Calibration_C1 + HP03_Calibration_C3_Mod * dUT;
	
	/* X = Sens * (D1 - 7168) / 2^14 - Off */
	X = Sens * ((double)AltitudeReading - 7168.0) / 16384.0 - Off;
	
	/* P = X * 100 / 2^5 + C7*10 */
	Pressure = X / 32.0 + HP03_Calibration_C7_Mod;

	return HP03_CalcAltitude(Pressure);
}


/*
 * NAME:        HP03_GetTemperatureFactor
 * ACTION:      Get the current temperature as a "dUT" factor
 * PARAMETERS:  
 * RETURNS:     double - the dUT factor or ALT_ERROR on error
 */
 
double HP03_GetTemperatureFactor()
{
    unsigned short TemperatureReading;
    double DTemperatureReading;
    double e, f;
    int Result;

    /* raise XCLR to enable measurement */
	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, HP03D_XCLR);
    
    /* send temperature measurement command */
    if (HP03_I2C_Measure(HP03_TEMPERATURE_SUBTYPE) != I2C_ERROR_OK)
    {
	    AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, HP03D_XCLR);
        return -1;
    }
            
    /* wait for 40ms */
    vTaskDelay(40/portTICK_RATE_MS);
    
    /* request result */
    Result = HP03_I2C_Result(&TemperatureReading);
    
    /* drop XCLR */
	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, HP03D_XCLR);

    /* fail if we need to */
    if (Result != I2C_ERROR_OK)
        return -1;
	
	/* calculate the dUT factor */
	DTemperatureReading = (double)TemperatureReading;
    e = DTemperatureReading - HP03_Calibration_C5;
    f = e * e / 16384.0;

	if (e >= 0)
    {
        /* dUT = D2 - C5 - ((D2 - C5) / 2^7) * ((D2 - C5) / 2^7) * A / 2^C */
    	return e - f * HP03_Calibration_AC;
    }
    else
    {
        /* dUT = D2 - C5 - ((D2 - C5) / 2^7) * ((D2 - C5) / 2^7) * B / 2^C */
    	return e - f * HP03_Calibration_BC;
    }
}


/*
 * NAME:        HP03_CalcTemperature
 * ACTION:      Calculate the current temperature from a "dUT"
 * RETURNS:     double - the temperature in degrees celcius
 *                    or ALT_ERROR on error
 */
 
double HP03_CalcTemperature(double dUT)
{
    /* make sure we've got the calibration data */
    if (!HP03_GotCalibrationData)
        HP03_GetCalibrationData();

    /* calculate the actual temperature */
    /* T = 250 + dUT * C6 / 2^16 - dUT / 2^D */
    return (250.0 + dUT * HP03_Calibration_C6_Mod - dUT / HP03_Calibration_D_Mod) / 10.0;
}


/*
 * NAME:        HP03_GetTemperature
 * ACTION:      Get the current temperature only
 * RETURNS:     double - the temperature in degrees celcius
 *                    or ALT_ERROR on error
 */
 
double HP03_GetTemperature()
{
    double dUT;

    /* get the dUT factor */
	dUT = HP03_GetTemperatureFactor();
	if (dUT < 0)
	    return dUT;
    
    return HP03_CalcTemperature(dUT);
}

/* pressure lookup table */
static double PressureToAltitude[HP03_PRESSURE_ARRAY_MAX] = 
{
    -698.3, -620.1, -541.3, -462.0, -382.0, -301.5, -220.3, -138.5,  -56.0,   27.0,
    //1100    1090    1080    1070    1060    1050    1040    1030    1020    1010 hpa
    
     110.8,  195.3,  280.5,  366.4,  422.4,  540.3,  628.4,  717.2,  806.8,  897.2, 
    //1000     990     980     970     960     950     940     930     920     910 hpa
    
     988.5, 1080.5, 1173.4, 1267.1, 1361.7, 1457.2, 1553.7, 1651.0, 1749.4, 1848.6,  
    // 900     890     880     870     860     850     840     830     820     810 hpa
    
    1948.9, 2050.2, 2152.6, 2256.0, 2360.5, 2466.2, 2573.0, 2680.9, 2790.1, 2900.5,  
    // 800     790     780     770     760     750     740     730     720     710 hpa
    
    3012.1, 3125.1, 3239.4, 3355.1, 3472.1, 3590.6, 3710.6, 3832.2, 3955.3, 4080.0,  
    // 700     690     680     670     660     650     640     630     620     610 hpa
    
    4206.0, 4334.5, 4464.4, 4596.1, 4729.6, 4865.2, 5002.7, 5142.3, 5284.1, 5428.1,  
    // 600     590     580     570     560     550     540     530     520     510 hpa
    
    5574.4, 5723.1, 5874.2, 6028.0, 6184.4, 6343.6, 6505.6, 6670.7, 6839.0, 7010.5,  
    // 500     490     480     470     460     450     440     430     420     410 hpa
    
    7185.4, 7363.9, 7546.1, 7732.3, 7922.6, 8117.2, 8316.4, 8520.4, 8729.4, 8943.8
    // 400     390     380     370     360     350     340     330     320     310 hpa
};


/*
 * NAME:    HP03_CalcAltitude
 * ACTION:  This function calculates the altitude by linearly interpolating the
 *          supplied table.  The table contains altitude values, but the table
 *          is actually indexed by pressure values.  So the algorithm must first
 *          find the first table entry that has a lower pressure value than the 
 *          current pressure reading.  The actual altitude is somewhere between 
 *          that table value and the next one.   Since there are only 10hpa 
 *          difference between table values, the fractional value is easy to 
 *          calculate.  That value is multiplied by the difference in altitude 
 *          between the table values to find the altitude amount that is above 
 *          the lowest of the table values.  Simply add those two together and
 *          the altitude is achieved.  Special care was taken to ensure that
 *          all long integer calculation retained two decimals of precision.  
 *          Therefore, each step in the algorithm will be 100 times the actual 
 *          value.
 * PARAMETERS: double Pressure - the pressure in Hpa
 * RETURNS: double - the altitude in metres
 */
 
double HP03_CalcAltitude(double Pressure)
{
    int Index;
    double PressureDifference;
    double AltitudeDifference;
    
    /* select the appropriate index into the altitude array */
    Index = (HP03_MAX_PRESSURE - (int)Pressure) / HP03_PRESSURE_INC + 1;
    if (Index >= HP03_PRESSURE_ARRAY_MAX)
        Index = HP03_PRESSURE_ARRAY_MAX-1;
    
    /* linear interpolation */
    PressureDifference = Pressure - (HP03_MAX_PRESSURE - Index * HP03_PRESSURE_INC);
    AltitudeDifference = PressureToAltitude[Index] - PressureToAltitude[Index-1];
    
    return PressureToAltitude[Index] - PressureDifference / HP03_PRESSURE_INC * AltitudeDifference;
}
